#include "ros/ros.h"
#include <math.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <signal.h>
#include <std_msgs/String.h>
#include<tf/transform_broadcaster.h>

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>

//#include<stdio.h>
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/Float64Stamped.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
 #include <webots_ros/robot_get_device_list.h>
#include <webots_ros/get_int.h>
#include <webots_ros/get_float.h>

#define TIME_STEP 32
#define NMOTORS 4
#define SENSORS 4
#define MAX_SPEED 10
 #define SUITABLE_SPEED 1
#define A_POSITION 0.2
#define WHEEL_DISTANCE 0.3
#define WHEEL_H 0.36
#define TRACK_TIME_STEP 0.1
using namespace std;

ros::NodeHandle *n;
static int controllerCount;

ros::ServiceClient time_step_client;
webots_ros::set_int time_step_srv;

static double accelerometerValues[3] = {0, 0, 0};
static double GPSValues[3] = {0, 0, 0};
static double inertialUnitValues[4]={0,0,0,0};
double velocity[2] ;
double x;
double y;
double theta;
double vr ;
double wr ;
double v_x;
double v_y;
double v_theta;
double v_xr;
double v_yr;
double v_thetar;
double last_vx = 0.2;
double last_vtheta = 0.1;
double v_xd;
double v_thetad;
static std::vector<std::string> controllerList; 
ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;
 
std::string ROS_NODE_NAME = "ros_test";
static const char *motorNames[NMOTORS] ={"rotational__z11", "rotational_z12","rotational__z212","rotational__z222"};//匹配之前定义好的电机name
//static const char *sensorNames[SENSORS]={"position_z11","position_z12","position_z212","position_z222"};
double speeds[NMOTORS];
float positions[4];  //四个轮子位置
float sensor_positions_val=0.0;

// callback functions
void controller2Callback(const geometry_msgs::Point::ConstPtr &values)
{
    v_xr = values->x;
    v_thetar = values->z;
	ROS_INFO("target linear speed and augler speed: %lf,%lf", v_xr,v_thetar);
}

void controllerNameCallback(const std_msgs::String::ConstPtr &name)
{ 
	controllerCount++; 
	controllerList.push_back(name->data);
	ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

void positionsensorCallback(const webots_ros::Float64Stamped::ConstPtr &value)
{
	for(int i=0;i<SENSORS;i++)
	{
	positions[i] = value->data;
	}
	//ROS_INFO("%f%f%f%f",positions[0], positions[1], positions[2], positions[3]);
}

void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) {
  accelerometerValues[0] = values->linear_acceleration.x;
  accelerometerValues[1] = values->linear_acceleration.y;
  accelerometerValues[2] = values->linear_acceleration.z;

  ROS_INFO("Accelerometer values are x=%f y=%f z=%f (time: %d:%d).", accelerometerValues[0], accelerometerValues[1],
           accelerometerValues[2], values->header.stamp.sec, values->header.stamp.nsec);
  //callbackCalled = true;
}

void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) {
  GPSValues[0] = values->point.x;
  GPSValues[1] = values->point.y;
  GPSValues[2] = values->point.z;
  x = GPSValues[0];
  y = GPSValues[1];
//  ROS_INFO("GPS values are x=%f y=%f z=%f (time: %d:%d).", GPSValues[0], GPSValues[1], GPSValues[2], values->header.stamp.sec,
  //         values->header.stamp.nsec);
 // callbackCalled = true;
}

void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) {
  ROS_INFO("GPS speed is: %fkm/h (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec);
  //callbackCalled = true;
}

void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) {
  inertialUnitValues[0] = values->orientation.x;
  inertialUnitValues[1] = values->orientation.y;
  inertialUnitValues[2] = values->orientation.z;
  inertialUnitValues[3] = values->orientation.w;

    double siny = 2 * (inertialUnitValues[2] * inertialUnitValues[3] + inertialUnitValues[0] * inertialUnitValues[1]);
    double cosy = 1 - 2 * (inertialUnitValues[1] * inertialUnitValues[1] + inertialUnitValues[2] * inertialUnitValues[2]);
    theta = std::atan2(siny, cosy);

 // ROS_INFO("Inertial unit values (quaternions) are x=%f y=%f z=%f w=%f (time: %d:%d).", inertialUnitValues[0],
  //         inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[2], values->header.stamp.sec,
  //         values->header.stamp.nsec);
  //callbackCalled = true;
}

void velCallback(const geometry_msgs::Twist::ConstPtr &values)
{
    v_x = values->linear.x;
    v_y = values->linear.y;
    v_theta = values->angular.z;                //动力学控制加在这块
    ROS_INFO("Speeds: vx=%.2f[m/s] vy=%.2f[m/s] vth=%.2f[rad/s]\n", v_x, v_y, v_theta);
   //运动学逆解得到每个轮子的目标速度
    velocity[0] = v_x-0.5*WHEEL_DISTANCE*v_theta;           //左后轮
    velocity[1] = v_x+0.5*WHEEL_DISTANCE*v_theta;         //右后轮
    velocity[2] = v_theta*sqrt(WHEEL_H*WHEEL_H+(v_x/v_theta-0.5*WHEEL_DISTANCE)*(v_x/v_theta-0.5*WHEEL_DISTANCE));    //左前轮速度
    velocity[3] = v_theta*sqrt(WHEEL_H*WHEEL_H+(v_x/v_theta-0.5*WHEEL_DISTANCE)*(v_x/v_theta+0.5*WHEEL_DISTANCE));   //右前轮速度
	velocity[4] = atan(WHEEL_H/(v_x/v_theta-0.5*WHEEL_DISTANCE));      //左前角速度
    velocity[5] = atan(WHEEL_H/(v_x/v_theta+0.5*WHEEL_DISTANCE));    //右前角速度
	

    for (int i = 0; i < NMOTORS; ++i) {
		ros::ServiceClient set_velocity_client;
		webots_ros::set_float set_velocity_srv;
		set_velocity_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
																+ std::string(motorNames[i]) + std::string("/set_velocity"));   
		set_velocity_srv.request.value = velocity[i];
		set_velocity_client.call(set_velocity_srv);
 	}
}

//运动学正解
 void Positive_solution(double vl, double vr)
 {
	v_x = (vl+vr)/2;
	v_y = 0.0;
	v_theta = (vr-vl)/WHEEL_DISTANCE;
 }

/**************************************************************************
Function: Floating-point data calculates the absolute value
Input   : float
Output  : The absolute value of the input number
函数功能：双精度浮点型数据计算绝对值
入口参数：浮点数
返回  值：输入数的绝对值
**************************************************************************/
double float_abs(double insert)
{
	if(insert>=0) return insert;
	else return -insert;
}

/**************************************************************************
函数功能：符号函数 
入口参数：函数自变量 
返回  值：计算结果
**************************************************************************/
double sign(double s)
{
	double temp;
	if (s<0) temp=-1.0;
	else temp=1.0;
	return temp;
 } 

/**************************************************************************
函数功能：fal非线性误差函数 
入口参数：误差值 
返回  值：计算结果 
**************************************************************************/
double falfunction (double error)
{
	double fe, sigma=0.25, delta=0.01, aerror,serror;
	aerror=float_abs(error);
	serror=sign(error);
	if(aerror>delta) fe=serror*pow(aerror,sigma) ;
	else fe=error/pow(delta,(1-sigma));
	return fe;
}

/**************************************************************************
函数功能：ADRC控制器
入口参数：实际速度，外环目标速度
返回  值：内环目标速度
**************************************************************************/
double ADRC_A(double Encoder, double Target)
{
	static double Pwm,je,de,dt,Last_Bias,Last_target ,Last_z21,ez11,s,Bias_A,z11A,z21A, v_d;   //eso
	//static float Bias,Pwm,je,de,Last_Bias;
	double T=0.1, h=0.001, Velocity_k3=0.1;
	int b1=4, p1=300, p2=1000, Velocity_k1=25, Velocity_k2=25;
	 Bias_A=Target-Encoder; //Calculate the deviation //计算偏差
	 je+=T*falfunction(Bias_A);
	dt=(Target-Last_target)/T;
	// de=Bias_AA-Last_Bias;
	 //je+=Bias_AA;
	 s=Bias_A+Velocity_k1*je;
	 ez11=z11A-Encoder;
	 z11A+=h*(Last_z21-p1*ez11+b1*Pwm);
	 z21A+=-h*p2*falfunction(ez11);
	// Pwm=Velocity_k1*Bias_AA+Velocity_k2*je+Velocity_k3*de;          //PID控制
//	 Pwm=(dt-z21+Velocity_KP*Bias+Velocity_KI*je)/b1;    //pi+eso
	 Pwm=(dt-z21A+Velocity_k1*falfunction(Bias_A)+Velocity_k2*s+Velocity_k3*sign(s))/b1;                  // ADRC
     v_d = Encoder + h*(b1* Pwm + z21A);
	 if(v_d>30)v_d=2;
	 if(v_d<-30)v_d=-2;
	 Last_Bias=Bias_A; //Save the last deviation //保存上一次偏差 
	Last_target=Target;
	Last_z21=z21A;
	 return v_d; 
}

double ADRC_B(double Encoder, double Target)
{
	static double Pwm,je,de,dt,Last_Bias,Last_target ,Last_z22,ez12,s,Bias_B,z12A,z22A, w_d;   //eso
	//static float Bias,Pwm,je,de,Last_Bias;
	double T=0.1, h=0.001, Velocity_k3=0.1;
	int b2=4.2, p1=300, p2=1000, Velocity_k1=25, Velocity_k2=25;
	 Bias_B=Target-Encoder;                                                      //Calculate the deviation //计算误差
	 je+=T*falfunction(Bias_B);
	dt=(Target-Last_target)/T;
	// de=Bias_AA-Last_Bias;
	 //je+=Bias_AA;
	 s=Bias_B+Velocity_k1*je;
	 ez12=z12A-Encoder;
	 z12A+=h*(Last_z22-p1*ez12+b2*Pwm);
	 z22A+=-h*p2*falfunction(ez12);
	// Pwm=Velocity_k1*Bias_AA+Velocity_k2*je+Velocity_k3*de;          //PID控制
//	 Pwm=(dt-z21+Velocity_KP*Bias+Velocity_KI*je)/b1;                                  //pi+eso
	 Pwm=(dt-z22A+Velocity_k1*falfunction(Bias_B)+Velocity_k2*s+Velocity_k3*sign(s))/b2;                     //ADRC
     w_d = Encoder + h*(b2* Pwm + z22A);
	 if(w_d>30)w_d=2;
	 if(w_d<-30)w_d=-2;
	 Last_Bias=Bias_B; //Save the last deviation //保存上一次偏差 
	Last_target=Target;
	Last_z22=z22A;
	 return w_d;  
}

void updateSpeed()
{
	v_x = v_xr;
	v_theta = v_thetar;

//运动学逆解得到每个轮子的目标速度
    speeds[0] = (v_x-0.5*WHEEL_DISTANCE*v_theta)/0.0625;           //左后轮
    speeds[1] = (v_x+0.5*WHEEL_DISTANCE*v_theta)/0.0625;         //右后轮
	speeds[2] = atan(WHEEL_H/(v_x/v_theta-0.5*WHEEL_DISTANCE));      //左前角速度
    speeds[3] = atan(WHEEL_H/(v_x/v_theta+0.5*WHEEL_DISTANCE));    //右前角速度
	// speeds[0] = ADRC_A(velocity[0],speeds[0]);
	// speeds[1] = ADRC_B(velocity[1],speeds[1]);    //ADRC内环	
//  //set torque // 力矩控制
// 	for (int i = 0; i < 4; ++i) 
// 	{
// 		ros::ServiceClient set_torque_client;
// 		webots_ros::set_float set_torque_srv;
// 		set_torque_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
// 																+ std::string(motorNames[i]) + std::string("/set_torque"));   
// 		set_torque_srv.request.value = torques[i];
// 		set_torque_client.call(set_torque_srv);
// 		if (set_torque_client.call(set_torque_srv) && set_torque_srv.response.success)
//     	ROS_INFO("Torque set to 0.2.");
//   		else
//     	ROS_ERROR("Failed to call service set_torque on motor.");
//  	}

	//set velocity for rear wheels
	for (int i = 0; i < 2; ++i) 
	{
		ros::ServiceClient set_velocity_client;
		webots_ros::set_float set_velocity_srv;
		set_velocity_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
																+ std::string(motorNames[i]) + std::string("/set_velocity"));   
		set_velocity_srv.request.value = speeds[i];
		set_velocity_client.call(set_velocity_srv);
 	}
 //set position for front wheels
 	for (int i = 2; i < NMOTORS; ++i) 
	{
		ros::ServiceClient set_position_client;
		webots_ros::set_float set_position_srv;
		set_position_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
																+ std::string(motorNames[i]) + std::string("/set_position"));   
		set_position_srv.request.value = speeds[i];
		set_position_client.call(set_position_srv);
	 }

//get velocity
	// for(int i = 0;i<2 ; ++i)
	// {
	// 	ros::ServiceClient get_velocity_client;
	// 	webots_ros::get_float get_velocity_srv;
	// 	get_velocity_client = n->serviceClient<webots_ros::get_float>(ROS_NODE_NAME+std::string("/")
	// 																+ std::string(motorNames[i])  + std::string("/get_velocity"));
	// 	get_velocity_client.call(get_velocity_srv);
	// 	velocity[i] = get_velocity_srv.response.value;
	// 	ROS_INFO("Velocity for rotational_motor is %f.", velocity[i]); //打印验证
	// 	get_velocity_client.shutdown();
	// 	time_step_client.call(time_step_srv);
	// }

}//将速度请求以set_float的形式发送给set_velocity_srv

void Vwmr_controller()
{
	ros::ServiceClient set_GPS_client;
  	webots_ros::set_int GPS_srv;
	ros::Subscriber sub_GPS_32;

	ros::ServiceClient set_Imu_client;
  	webots_ros::set_int Imu_srv;
    ros::Subscriber sub_Imu;

    ros::Subscriber sub_controller2;
	sub_controller2 = n->subscribe("/controller2",1,controller2Callback);

    ros::Publisher odom_pub = n->advertise<nav_msgs::Odometry>(ROS_NODE_NAME  + "/odom", 50);
    tf::TransformBroadcaster odom_broadcaster;

    //enable gps
  	set_GPS_client = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME + "/gps/enable");
	GPS_srv.request.value = TIME_STEP;
	if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success)
	{
		ROS_INFO("GPS enabled.");
		sub_GPS_32 = n->subscribe(ROS_NODE_NAME + "/gps/values", 1, GPSCallback);
	}

     //enable Imu
	set_Imu_client = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME + "/inertial_unit/enable");
	Imu_srv.request.value = TIME_STEP;
	if (set_Imu_client.call(Imu_srv) && Imu_srv.response.success)
	{
		ROS_INFO("Imu enabled.");
		sub_Imu = n->subscribe(ROS_NODE_NAME + "/inertial_unit/quaternion", 1, inertialUnitCallback);
	}

    ros::Rate loop_rate(1 / TRACK_TIME_STEP);
	//main loop
	while (ros::ok())
	{
		ros::spinOnce();
		if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
		{  
			ROS_ERROR("Failed to call service time_step for next step.");     
		}   
		updateSpeed();
		//ROS_INFO("target linear speed and augler speed: %lf,%lf", v_xr,v_thetar);
 		//ROS_INFO("linear speed and augler speed: %lf,%lf", v_x,v_theta);
		ROS_INFO("left and right wheels' speed: %lf,%lf,%lf,%lf", speeds[0],speeds[1],speeds[2],speeds[3]);


        // Publishing Odometry Information over ROS
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);

        // first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        // send the transform
        odom_broadcaster.sendTransform(odom_trans);

        // next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = ros::Time::now();
        odom.header.frame_id = "odom";

        // set the position
	//	ROS_INFO("x and y: %lf,%lf", x,y);
 	//	ROS_INFO("theta: %lf", theta);
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = theta;
        odom.pose.pose.orientation = odom_quat;

        // set the velocity
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = v_x;
        odom.twist.twist.linear.y = v_y;
        odom.twist.twist.angular.z = v_theta;

        // publish the message
        odom_pub.publish(odom);

		loop_rate.sleep();
	}
}

void quit(int sig) 
{
	ROS_INFO("User stopped the 'ros_test' node.");
	timeStepSrv.request.value = 0; 
	timeStepClient.call(timeStepSrv); 
	ros::shutdown();
	exit(0);
}

int main(int argc, char **argv)
{
 	std::string controllerName;
 	// create a node named 'ros_test' on ROS network
	ros::init(argc, argv, ROS_NODE_NAME,ros::init_options::AnonymousName);
	n = new ros::NodeHandle;  
	signal(SIGINT, quit);
 	//subscribe to the topic model_name to get the list of availables controllers
	ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
	while (controllerCount == 0 || controllerCount <nameSub.getNumPublishers())
		{
			ros::spinOnce();
 		} 
	ros::spinOnce();
 
	timeStepClient = n->serviceClient<webots_ros::set_int>(ROS_NODE_NAME+"/robot/time_step");
	timeStepSrv.request.value = TIME_STEP;


	//if there is more than one controller available, it let the user choose 
	if (controllerCount == 1)   
		controllerName = controllerList[0];
	else 
	{
		int wantedController = 0;
		std::cout << "Choose the # of the controller you want touse:\n";   
		std::cin >> wantedController;   
		if (1 <= wantedController && wantedController <= controllerCount)
		controllerName = controllerList[wantedController - 1];   
		else
		{
		ROS_ERROR("Invalid number for controller choice.");
		return 1;
		}
	} 
	ROS_INFO("Using controller: '%s'", controllerName.c_str());
	// leave topic once it is not necessary anymore
	nameSub.shutdown();
 
	// init motors 
for (int i = 0; i < 2; ++i) 
{  
		// position，发送电机位置给wheel1-2，速度控制时设置为缺省值INFINITY   
		ros::ServiceClient set_position_client;   
		webots_ros::set_float set_position_srv;   
		set_position_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
																+ std::string(motorNames[i]) + std::string("/set_position"));   
		set_position_srv.request.value = INFINITY;
		if (set_position_client.call(set_position_srv) &&set_position_srv.response.success)     
			ROS_INFO("Position set to INFINITY for motor %s.",motorNames[i]);   
		else     
			ROS_ERROR("Failed to call service set_position on motor %s.",motorNames[i]);

			
		// speed，发送电机速度给wheel1-2 
		ros::ServiceClient set_velocity_client;
		webots_ros::set_float set_velocity_srv;   
		set_velocity_client =
		n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
		+ std::string(motorNames[i]) + std::string("/set_velocity"));   
		set_velocity_srv.request.value = 0.0;   
		if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
			ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);   
		else     
			ROS_ERROR("Failed to call service set_velocity on motor %s.",
		motorNames[i]);
}  
for (int i = 2; i < 4; ++i) 
{  
		// position，发送电机位置给wheel3-4   
		ros::ServiceClient set_position_client;   
		webots_ros::set_float set_position_srv;   
		set_position_client = n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
																+ std::string(motorNames[i]) + std::string("/set_position"));   
		set_position_srv.request.value = 0.0;
		if (set_position_client.call(set_position_srv) &&set_position_srv.response.success)     
			ROS_INFO("Position set to 0.0 for motor %s.",motorNames[i]);   
		else     
			ROS_ERROR("Failed to call service set_position on motor %s.",motorNames[i]);

			
		// speed，发送电机速度给wheel3-4，位置控制时设置为缺省值INFINITY   
		ros::ServiceClient set_velocity_client;
		webots_ros::set_float set_velocity_srv;   
		set_velocity_client =
		n->serviceClient<webots_ros::set_float>(ROS_NODE_NAME+std::string("/")
		+ std::string(motorNames[i]) + std::string("/set_velocity"));   
		set_velocity_srv.request.value = INFINITY;   
		if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
			ROS_INFO("Velocity set to INFINITY for motor %s.", motorNames[i]);   
		else     
			ROS_ERROR("Failed to call service set_velocity on motor %s.",
		motorNames[i]);
}  

Vwmr_controller();

timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown(); 
return 0;
}
